/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "OccuGUI.h"
#include "BetBdtLocalizer.h"
#include "ownutils.h"
#include "CParticleFilter.h"

#define DEFAULT_J2B2_MEAS_FILE "../../../data/j2b2_measurement.txt"
#define DEFAULT_MAP_FILE "../../res/autmap-strip.txt"

/**
* TODO: 
* - Continous Localization (initial estimate + localizing one scan at a time)
* - Using scans to update the SLAM maps after local map registration
* - Global map localization in combination of map registration
* - "Multi SLAM" case (N overlapping SLAM maps fitting)
* - Loop Closure 
* - Using the output hyphotheses as MCL initialization
* - Scan match error tolerance
* - Adding trajectory likelihood to B&B
*/

void timer(bool isStart){
		static long int ss1,us1;
		long int ss2,us2,sec,us,ms;
		float fms;
		
		if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
		else{
			ownTime_GetTimeOfDay(&ss2,&us2);
			sec = ss2-ss1;
			us = us2-us1;
			ms = sec*1000+us/1000;
			fms=(float)sec*1000.0+(float)us/1000.0;
			fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		}
}

/// For reading J2B2 data from filet
int readScanLine181(FILE *f,Grid::TScan &meas, float &x,float &y,float &a){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&x,&y,&a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						//fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, mcl::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}


void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/**
* Localizes with EnvironmentMeasurement2D data. With initial guess from MCL and 10m x 10m x 360 search area 
* @param *env_file_name The name of the EnvironmentMeasurement2D file
*/
void globalLocalizationDemo(const char *env_file_name){
	CEnvironmentMeasurement2D meas;
	COccuGUI ogui(DEFAULT_MAP_FILE,0.05,0.4);
	fprintf(stderr,"Using log file: '%s'\n",env_file_name);
	FILE *fmeas = fopen(env_file_name,"rt");
	if(fmeas == NULL){
			fprintf(stderr,"Unable to open the measurement file!!!\n");
			exit(1);
	}
	
	CBetBdtLocalizer localizer(DEFAULT_MAP_FILE,0.05);
	CMatchStorage matches(500);
	float x,y,a;
	bool run = true;
	while(!feof(fmeas) && run){
		meas.fread(fmeas);
		x = meas.frame.x_ref; y = meas.frame.y_ref; a=meas.frame.a_ref;
		fprintf(stderr,"Got %d measurements from pose (%.3f,%.3f,%.3f) \n",meas.points.size(),meas.frame.x_ref,
						meas.frame.y_ref,meas.frame.a_ref );
		
		fprintf(stderr,"Plotting the original measurement\n");
		ogui.plotEnvironmentMeasurement2D(meas);
		
		meas.frame.setError(10,10,M_PI);	
		localizer.localize(meas,matches);
		matches.sort();
		run = ogui.plotMatchResultsWithPoints(matches,meas,x,y,a,0.5);
	}
}

/**
 * Localizes with EnvironmentMeasurement2D data (from J2B2 occu_slam)
 * Uses 10/15cm resolution data, with initial guess from MCL and 40m x 40m x 360 search area 
 */
void globalCoarseLocalizationDemo(const char *env_file_name){
		CEnvironmentMeasurement2D meas;
		COccuGUI ogui(DEFAULT_MAP_FILE,0.05,0.4);
	//ogui.generateOccuMeasurement(meas,50);
		fprintf(stderr,"Using log file: '%s'\n",env_file_name);
		FILE *fmeas = fopen(env_file_name,"rt");
		if(fmeas == NULL){
				fprintf(stderr,"Unable to open the measurement file!!!\n");
				exit(1);
		}
	
		CBetBdtLocalizer localizer(DEFAULT_J2B2_MEAS_FILE,0.05);
		CMatchStorage matches(500);
		float x,y,a;
		bool run = true;
		while(!feof(fmeas) && run){
				meas.fread(fmeas);
				x = meas.frame.x_ref; y = meas.frame.y_ref; a=meas.frame.a_ref;
				fprintf(stderr,"Got %d measurements from pose (%.3f,%.3f,%.3f) \n",meas.points.size(),meas.frame.x_ref,
								meas.frame.y_ref,meas.frame.a_ref );
		
				fprintf(stderr,"Plotting the original measurement\n");
				ogui.plotEnvironmentMeasurement2D(meas);
		
				meas.frame.setError(40,40,M_PI);	
				localizer.localize(meas,matches);
				matches.sort();
				run = ogui.plotMatchResultsWithPoints(matches,meas,x,y,a,0.5);
		}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/**
* Continous localization using B&B
* - Initial estimate from somewhere 
* - calculating pose after each measurement
* NOTE: Sensitivity to map and measurement difference 
* Does not work veri vell.
**/
void continousLocalizationDemo(const char *scan_file_name){
		CEnvironmentMeasurement2D meas;
		COccuGUI ogui(DEFAULT_MAP_FILE,0.05,0.4);

		fprintf(stderr,"Using log file: '%s'\n",scan_file_name);
		FILE *fmeas = fopen(scan_file_name,"rt");
		if(fmeas == NULL){
				fprintf(stderr,"Unable to open the measurement file!!!\n");
				exit(1);
		}
		FILE *f_out = fopen("test_measurement.txt","wt");
		
		CBetBdtLocalizer localizer(DEFAULT_MAP_FILE,0.05);
		CMatchStorage matches(500);
		
		Grid::TScan scan;
		scan.alloc(181);
		mcl::pose init_est,diff,pos;
		mcl::pose last,current;
		mcl::pose reference_pose, pose_now;
		
		fprintf(stderr,"WARNING: This initial pose will work only with ../../../data/j2b2_measurement.txt\n");
		init_est.set(10.2,-5.2,-0.521);
		pos = init_est;
		float search_x = .15;
		float search_y = .15;
		float search_a = 5.0*M_PI/180.0;

		bool run = true;
	  float x,y,a;
		
		readScanLine181(fmeas,scan,x,y,a); ///< read the first measurement
		readScanLine181(fmeas,scan,x,y,a); ///< read the first measurement
		last.set(x,y,a);
		TMeasurementPoint p;
		int cnt=0;
		while(run && !feof(fmeas)){
				for(int i=0;i<10;i++) readScanLine181(fmeas,scan,x,y,a);
				current.set(x,y,a);
				
				
				///Estimate of the current pose
				diff.setToDifferentialPose(current,last);
				fprintf(stderr,"Diff:(%.2f,%.2f,%.2f) ",diff.x,diff.y,diff.a );
				init_est = init_est.integrateDifferential(diff);
				pos = pos.integrateDifferential(diff);
				last=current;
				writeScanLine(f_out,scan, pos.x,pos.y,pos.a);
				fflush(f_out);				
				
				meas.frame.setError(search_x,search_y,search_a);
				//meas.frame.setError(0.5*diff.x,0.5*diff.y,0.1*diff.a);
				meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
				meas.clear();
				for(int i=0;i<scan.N;i++){
						p.x = scan.r[i]*cos(scan.a[i]);
						p.y = scan.r[i]*sin(scan.a[i]);
						meas.addPoint(p);
				}
				fprintf(stderr,"Initial = (%.2f,%.2f,%.2f)...\n ",init_est.x,init_est.y,init_est.a );
				localizer.localize(meas,matches);
				init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
				fprintf(stderr,"Corrected = (%.3f,%.3f,%.3f)\n", matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
				if(cnt%100==0) run = ogui.plotMatchResultsWithPoints(matches,meas,pos.x,pos.y,pos.a,0.5);
				cnt++;
		}
		fclose(f_out);
} 


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/**
 * Localizes with EnvironmentMeasurement2D data (from J2B2 occu_slam)
 * Uses 4cm resolution data, with initial guess from MCL and with variance estimate from MCL
 */
void mclLocalizationDemo(const char *env_file_name){
		CEnvironmentMeasurement2D meas;
		COccuGUI ogui(DEFAULT_MAP_FILE,0.05,0.4);
	//ogui.generateOccuMeasurement(meas,50);
		fprintf(stderr,"Using log file: '%s'\n",env_file_name);
		FILE *fmeas = fopen(env_file_name,"rt");
		if(fmeas == NULL){
				fprintf(stderr,"Unable to open the measurement file!!!\n");
				exit(1);
		}
	
		CBetBdtLocalizer localizer(DEFAULT_MAP_FILE,0.05);
		CMatchStorage matches(500);
		float x,y,a;
		bool run = true;
		while(!feof(fmeas) && run){
				meas.fread(fmeas);
				x = meas.frame.x_ref; y = meas.frame.y_ref; a=meas.frame.a_ref;
				fprintf(stderr,"Got %d measurements from pose (%.3f,%.3f,%.3f) \n",meas.points.size(),meas.frame.x_ref,
								meas.frame.y_ref,meas.frame.a_ref );
		
				fprintf(stderr,"Plotting the original measurement\n");
				ogui.plotEnvironmentMeasurement2D(meas);
				
				meas.frame.x_err = 3.0*sqrt(meas.frame.x_err);
				meas.frame.y_err = 3.0*sqrt(meas.frame.y_err);
				meas.frame.a_err = 3.0*sqrt(meas.frame.a_err);
				if(meas.frame.a_err<15.0*M_PI/180.0) meas.frame.a_err=25.0*M_PI/180.0;
				fprintf(stderr,"Initial Estimate (%.3f,%.3f,%.3f)...",x,y,a);
				localizer.localize(meas,matches);
				fprintf(stderr,"Corrected = (%.3f,%.3f,%.3f)\n", matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
				run = ogui.plotMatchResultsWithPoints(matches,meas,x,y,a,0.5);
		}
}


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///Test SLAM
void MLSLAMDemo(const char *env_file_name){
	CEnvironmentMeasurement2D meas;	
	CBetBdtLocalizer slammer(100,0.05); 
	float x=0,y=0;
	int cnt = 0;
	slammer.grid->setGridToValue(0.5);
	COccuGUI sgui(100,0.05,0.5); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
	fprintf(stderr,"Using log file: '%s'\n",env_file_name);
	
	FILE *fmeas = fopen(env_file_name,"rt");
	//FILE *fmeas = fopen("environment_measurement2D_slam_j2b2_success.txt","rt");
	if(fmeas == NULL){
			fprintf(stderr,"Unable to open the measurement file!!!\n");
			exit(1);
	}
	float search_x = 1.0;
	float search_y = 1.0;
	float search_a = 10.0*M_PI/180.0;
	
	CMatchStorage matches(10);
	bool run = true;
	
	mcl::pose init_est,diff;
	mcl::pose last,current;
	
	while(run){
			meas.fread(fmeas);
			current.set(meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref); ///< most recent MCL estimate
			
			if(cnt == 0){
					x = meas.frame.x_ref; y = meas.frame.y_ref;
					last.set(meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref);
					meas.frame.x_ref = 0;meas.frame.y_ref = 0;meas.frame.a_ref = 0;
					
					init_est.set(meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref);
			}else{
					diff=last;
					last = current;
					init_est = init_est.integrateDifferential(diff); /// from previous to current
					meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
					if(cnt == 1)	meas.frame.setError(search_x,search_y,2*search_a);			 
					else meas.frame.setError(search_x,search_y,2*search_a);
			}
			cnt ++;
			//slammer.setMeasurementToMap(meas);
			//sgui.setOccupancyGrid(slammer.grid);
			//sgui.plotMatchResults(matches,meas.frame.x_ref,meas.frame.y_ref,0);
			//run = sgui.plotEnvironmentMeasurement2D(meas);
			
			if(slammer.localize(meas,matches)){
					fprintf(stderr,"Trueth:(%.2f,%.2f,%.2f) est:(%.2f,%.2f,%.2f)\n",
									mx,my,ma,meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref);
					
					for(unsigned int i=0; i<matches.storage.size();i++){
							fprintf(stderr,"pose(%.3f,%.3f,%.3f) weight='%.3f' area(%.3f,%.3f,%.3f)\n",matches.storage[i].x,
											matches.storage[i].y,matches.storage[i].a*180./M_PI ,matches.storage[i].weight,matches.storage[i].ax,
											matches.storage[i].ay,matches.storage[i].aa);
					}
					meas.frame.x_ref = matches.storage[0].x;
					meas.frame.y_ref = matches.storage[0].y;
					meas.frame.a_ref = matches.storage[0].a;
					init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
					
					slammer.setMeasurementToMap(meas);
					sgui.setOccupancyGrid(slammer.grid);
					
					if(run) run = sgui.plotMatchResults(matches,matches.storage[0].x,
																						matches.storage[0].y,matches.storage[0].a);
			}else{
					sgui.setOccupancyGrid(slammer.grid);
					run = sgui.drawGrid();
			}
			
	}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///Test "Global" SLAM
/// FIXME: Not implemented
void GlobalSLAMDemo(){
		CEnvironmentMeasurement2D meas;	
		CBetBdtLocalizer slammer(120,0.05); 
		float x=0,y=0;
		int cnt = 0;
	
		COccuGUI sgui(120,0.05,0.5); ///< for visualizing the SLAM map
		float mx=0,my=0,ma=0;
	
		float search_x = 80.0;
		float search_y = 80.0;
		float search_a = M_PI;
		CMatchStorage matches(500);
		bool run = true;
		COccuGUI ogui("../res/autmap-strip.txt",0.05,0.5);
		while(run){
				run = ogui.generateOccuMeasurement(meas,100.0);
			
				if(cnt == 0){
						x = meas.frame.x_ref; y = meas.frame.y_ref;
						meas.frame.x_ref = 0;meas.frame.y_ref = 0;meas.frame.a_ref = 0;
				}else{
						meas.frame.x_ref-=x;meas.frame.y_ref-=y;
						mx = meas.frame.x_ref; my = meas.frame.y_ref;ma=meas.frame.a_ref;
						meas.frame.x_ref = 0;meas.frame.y_ref = 0;meas.frame.a_ref = 0; ///No prior knowledge
						
						meas.frame.setError(search_x,search_y,search_a);			 
				}
				cnt ++;
			//slammer.setMeasurementToMap(meas);
			//sgui.setOccupancyGrid(slammer.grid);
			//sgui.plotMatchResults(matches,meas.frame.x_ref,meas.frame.y_ref,0);
			
			
				if(slammer.localize(meas,matches)){
						fprintf(stderr,"Trueth:(%.2f,%.2f,%.2f) est:(%.2f,%.2f,%.2f)\n",
										mx,my,ma,meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref);
					
						for(unsigned int i=0; i<matches.storage.size();i++){
								fprintf(stderr,"pose(%.3f,%.3f,%.3f) weight='%.3f' area(%.3f,%.3f,%.3f)\n",matches.storage[i].x,
												matches.storage[i].y,matches.storage[i].a*180./M_PI ,matches.storage[i].weight,matches.storage[i].ax,
												matches.storage[i].ay,matches.storage[i].aa);
						}
						meas.frame.x_ref = matches.storage[0].x;
						meas.frame.y_ref = matches.storage[0].y;
						meas.frame.a_ref = matches.storage[0].a;
					
						slammer.setMeasurementToMap(meas);
						sgui.setOccupancyGrid(slammer.grid);
						if(run) run = sgui.plotMatchResults(matches,matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,0.25);
				}else{
						sgui.setOccupancyGrid(slammer.grid);
						run = sgui.drawGrid();
				}
			
		}
}


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///Test Local SLAM
void ML_Local_SLAMDemo(const char *scan_meas, const char *env_out_file, const char *scan_meas_out, bool visu){
		CEnvironmentMeasurement2D meas;	
		float grid_size = 80.0;
		CBetBdtLocalizer slammer(grid_size,0.04); 
		float x=0,y=0,a=0;
		int cnt = 0;
		Grid::TScan scan;
		scan.alloc(600);
		
		COccuGUI sgui(grid_size,0.04,1.0); ///< for visualizing the SLAM map
		float mx=0,my=0,ma=0;
		fprintf(stderr,"Using '%s' input file\n",scan_meas);
		//FILE *fmeas = fopen("../../../data/j2b2_measurement.txt","rt");
		FILE *fmeas = fopen(scan_meas,"rt");
	//FILE *fmeas = fopen("environment_measurement2D_slam_j2b2_success.txt","rt");
		if(fmeas == NULL){
				fprintf(stderr,"Unable to open the measurement file!!!\n");
				exit(1);
		}
		fprintf(stderr,"Using '%s' env2D output file\n",env_out_file);
		FILE *fenv_meas = fopen(env_out_file,"wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		fprintf(stderr,"Using '%s' scan output file\n",scan_meas_out);
		FILE *fmeas_out = fopen(scan_meas_out,"wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		
		
		
		float search_x = .5;
		float search_y = .5;
		float search_a = 20.0*M_PI/180.0;
	
		CMatchStorage matches(100);
		bool run = true;
	
		mcl::pose init_est,diff,pos;
		mcl::pose last,current;
		mcl::pose reference_pose, pose_now;
		
		std::vector< std::vector<float> > scans; ///All scans for previous map
		std::vector< mcl::pose > poses;    ///All poses for previous map
		std::vector<float> tmp_scan;
		
		///Read first measurement and put it into the map
		//for(int i = 0;i<100;i++) 
		readScanLine(fmeas,scan,last);
		
		//last.set(x,y,a);
		
		slammer.setMeasurementToMap(scan, 0, 0,  0, 0.99);
		
		//fprintf(stderr,"Scan has %d points\n",scan.N);
		//for(int i=0;i<scan.N;i++){
		//		fprintf(stderr,"'%.2f %.2f' ",scan.r[i],scan.a[i]);
		//}
		fprintf(stderr,"\n\n");
		slammer.tuneParameters(25.0,0.08);
		bool updated = false;
		
		while(run && !feof(fmeas)){
				for(int i = 0;i<1;i++) readScanLine(fmeas,scan,current);
				//current.set(x,y,a);
				
				///Save scans and poses 
				tmp_scan.clear();
				for(int i=0;i<scan.N;i++){
						tmp_scan.push_back(scan.r[i]);
				}
				scans.push_back(tmp_scan);
				
				
				diff.setToDifferentialPose(current,last);
				init_est = init_est.integrateDifferential(diff);
				last=current;
				
				///Put scan to env meas class
				TMeasurementPoint p;
				meas.points.clear();
				for(int i=0;i<scan.N;i++){
						if(scan.r[i]<25){
								p.x = scan.r[i]*cos(scan.a[i]);
								p.y = scan.r[i]*sin(scan.a[i]);
								meas.addPoint(p);
						}else{
								scan.r[i] = 0;
						}
				}
				meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
				meas.frame.setError(search_x,search_y,search_a);
				timer(true);
				if(slammer.localize(meas,matches)){
						matches.sort();
						/*for(unsigned int i=0; i<matches.storage.size()/10;i++){
								fprintf(stderr,"pose(%.3f,%.3f,%.3f) weight='%.5f' area(%.3f,%.3f,%.3f)\n",matches.storage[i].x,
												matches.storage[i].y,matches.storage[i].a*180./M_PI ,matches.storage[i].weight,matches.storage[i].ax,
												matches.storage[i].ay,matches.storage[i].aa);
						}*/
						meas.frame.x_ref = matches.storage[0].x;meas.frame.y_ref = matches.storage[0].y;
						meas.frame.a_ref = matches.storage[0].a;
						init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
						
						writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
						fprintf(stderr,"Pose now (%.2f %.2f %.2f) ",pose_now.x,pose_now.y,pose_now.a );
						
						poses.push_back(init_est); ///Save pose for this scan 
						
						slammer.setMeasurementToMap(scan,init_est.x,init_est.y,init_est.a,0.95);
						sgui.setOccupancyGrid(slammer.grid);
					
						if(run && updated){
								run = sgui.plotMatchResults(matches,matches.storage[0].x,
																						matches.storage[0].y,matches.storage[0].a);
								updated = false;
						}
				}else{
						fprintf(stderr,"WTF!!!!\n");
				}
				
				
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				pos = init_est;
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				if(toEdge < 10.0 || feof(fmeas)){
						//meas.clear();
						meas.setOccupancyGrid(slammer.grid, 0.8); ///< set grid to 2D measurement 
						meas.frame.setFrame(pos.x,pos.y,pos.a);		
						meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
						
						fprintf(stderr,"Saving Original map\n");
						meas.fprint(fenv_meas);        ///< Save to file
						fflush(fenv_meas);             ///< flush
						
						slammer.grid->setGridToValue(0.5); ///< Clear map
						reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
							
						pos.set(0,0,0); ///< initialize pose
						init_est.set(0,0,0);
						
						for(unsigned int i=1;i<scans.size()-2;i+=1){
								for(int j=0;j<scan.N;j++){
										scan.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
										if(scan.r[j]>15.0) scan.r[j]=0; 
								}
								diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
								slammer.setMeasurementToMap(scan,diff.x,diff.y,diff.a,0.95,false);
						}
						
						slammer.updateDistTransform();
						sgui.setOccupancyGrid(slammer.grid);
						sgui.save();
						if(visu) updated = true;
						scans.clear();
						poses.clear();
				}
				timer(false);
				cnt++;
		}
		fclose(fenv_meas);
		fclose(fmeas_out);
}



///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/** 
* Test Very Local SLAM = Scan Match
* A "real-time" version of the B&B 
* Localizes on a small local map.
*
* @param *scan_meas odometry/scan input file name (currently J2B2)  
* @param *env_out_file CEnvironmentMeasurement2D output file
* @param *scan_meas_out outputs the scan/odo measurement file
* @param visu set true to use visualization
**/
void ScanMatchDemo(const char *scan_meas, const char *env_out_file, const char *scan_meas_out, bool visu){
	CEnvironmentMeasurement2D meas;	
	float grid_size = 130.0;
	float maxLaserDistance = 65.0;
	CBetBdtLocalizer slammer(grid_size,0.04); 
	float x=0,y=0,a=0;
	int cnt = 0;
	Grid::TScan scan;
	scan.alloc(600);
		
	//COccuGUI sgui(grid_size,0.06,1.0); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
		FILE *fmeas = fopen(scan_meas,"rt");
	//FILE *fmeas = fopen("j2b2_measurement_labrat.txt","rt");
	if(fmeas == NULL){
		fprintf(stderr,"Unable to open the measurement file '%s'!!!\n",scan_meas);
		exit(1);
	}
		
	FILE *fenv_meas = fopen(env_out_file,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
		
	FILE *fmeas_out = fopen(scan_meas_out,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	float search_x = .5;
	float search_y = .5;
	float search_a = 20.0*M_PI/180.0;
	
	CMatchStorage matches(100);
	bool run = true;
	
	mcl::pose init_est,diff,pos;
	mcl::pose last,current;
		
	mcl::pose reference_pose, pose_now;
	
	std::vector< std::vector<float> > scans; ///All scans for previous map
	std::vector< mcl::pose > poses;    ///All poses for previous map
	std::vector<float> tmp_scan;
		
		///Read first measurement and put it into the map
	//for(int i = 0;i<200;i++) 
	readScanLine(fmeas,scan,last);
		
	//last.set(x,y,a);
		
	slammer.setMeasurementToMap(scan, 0, 0,  0, 0.99);
	writeScanLine(fmeas_out,scan ,0.f,0.f,0.f);
		
	fprintf(stderr,"Scan has %d points\n",scan.N);
	for(int i=0;i<scan.N;i++){
		fprintf(stderr,"'%.2f %.2f' ",scan.r[i],scan.a[i]);
	}
	fprintf(stderr,"\n\n");
	slammer.tuneParameters(15.0,0.12);
	bool updated = false;
		
	while(run){
		for(int i = 0;i<1;i++) readScanLine(fmeas,scan,current);
		//current.set(x,y,a);
				
				///Save scans and poses 
		tmp_scan.clear();
		for(int i=0;i<scan.N;i++){
			tmp_scan.push_back(scan.r[i]);
		}
		scans.push_back(tmp_scan);
				
				
		diff.setToDifferentialPose(current,last);
		#warning "REMOVE THIS LATER"
		diff.set(0,0,0);
		init_est = init_est.integrateDifferential(diff);
		last=current;
				
		///Put scan to env meas class
		TMeasurementPoint p;
		meas.points.clear();
		for(int i=0;i<scan.N;i++){
			if(scan.r[i]<maxLaserDistance){
				p.x = scan.r[i]*cos(scan.a[i]);
				p.y = scan.r[i]*sin(scan.a[i]);
				meas.addPoint(p);
			}else{
				scan.r[i] = 0;
			}
		}
		meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
		meas.frame.setError(search_x,search_y,search_a);
		
		timer(true);
						
		if(slammer.localize(meas,matches)){
			matches.sort();
			meas.frame.x_ref = matches.storage[0].x; meas.frame.y_ref = matches.storage[0].y; meas.frame.a_ref = matches.storage[0].a;
			init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						
			pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
			writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
			fprintf(stderr,"Pose now (%.2f %.2f %.2f) ",pose_now.x,pose_now.y,pose_now.a );
			poses.push_back(init_est); ///Save pose for this scan 
						
			slammer.setMeasurementToMap(scan,init_est.x,init_est.y,init_est.a,0.99);
			//sgui.setOccupancyGrid(slammer.grid);
			/*		
			if(run && updated){
				run = sgui.plotMatchResults(matches,matches.storage[0].x,
																		matches.storage[0].y,matches.storage[0].a);
				updated = false;
			}*/
		}else{
			fprintf(stderr,"WTF!!!!\n");
		}
				
				
		////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
		////////////////////////////////////////////////////////////////////////////
		pos = init_est;
		float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
		float toEdge;
		toEdge = (grid_size/2.0)-toPos;			
		if(toEdge < 64.0 || feof(fmeas)){
			meas.setOccupancyGrid(slammer.grid, 0.8); ///< set grid to 2D measurement 
			meas.frame.setFrame(pos.x,pos.y,pos.a);		
			meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
						
			fprintf(stderr,"Saving Original map\n");
			meas.fprint(fenv_meas);        ///< Save to file
			fflush(fenv_meas);             ///< flush
			//slammer.grid->setGridToValue(0.5); ///< Clear map
			
			reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
			pos.set(0,0,0); ///< initialize pose
			init_est.set(0,0,0);
			
			for(unsigned int i=1;i<scans.size()-2;i+=1){
				for(int j=0;j<scan.N;j++){
					scan.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
					if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
				slammer.setMeasurementToMap(scan,diff.x,diff.y,diff.a,0.99,false);
			}
			slammer.updateDistTransform();
			
// 			if(visu){
// 				sgui.setOccupancyGrid(slammer.grid);
// 				//sgui.save();
// 				updated=true;
// 			}
			scans.clear();
			poses.clear();
		}
		timer(false);
		cnt++;	
	}
}





///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void printUsage(){
		fprintf(stderr,"\n--------------------------------------------------------------------\n");
		fprintf(stderr,"Branch and Bound Localizer Demo Application\n");	
		fprintf(stderr,"\nThis application contains different use cases for BetB_dt Localizer\n");
		fprintf(stderr,"The different cases uses 'simulated' data which is requested as user input.\n");
		fprintf(stderr,"\nUsgage ./bbdt [num]\n");
		fprintf(stderr,"[num] a demo number:\n");
		fprintf(stderr,"0: Global localization demo with (10x10x360) area [env_meas2D_file_name]\n");
		fprintf(stderr,"1: Global localization demo with (40x40x360) area [env_meas2D_file_name]\n");
		fprintf(stderr,"2: MCL Localizer demo. [env_meas2D_file_name]\n");
		fprintf(stderr,"3: ML-SLAM Demo. [env_meas2D_file_name]\n");
		fprintf(stderr,"4: 	ML_Local_SLAMDemo. [scan_meas] [env_out_file]\n");
		fprintf(stderr,"5: 	ScanMatch Demo. 	 [scan_meas] [env_out_file] [scan_meas_out]\n");
		fprintf(stderr,"6: 	Continous Localization Demo.- runs with default log file\n");
		fprintf(stderr,"--------------------------------------------------------------------\n\n\n");
}


int main(int argc, char *argv[]){
	  int demo = 0;
		srand(time(NULL));
		const char *scan_meas=DEFAULT_J2B2_MEAS_FILE;
		const char *env_out_file = "environment_measurement2D_out.txt";
		const char *env_file = "environment_measurement2D.txt";
		const char *scan_meas_out = "bbdt_measurement.txt";		
		
		printUsage();
		if(argc < 2){
				printUsage();
				return 0;
		}
		else{
				demo = atoi(argv[1]);
		}
		
		switch(demo){
				case 0:
						fprintf(stderr,"Global localization demo with (10x10x360) area\n");
						if(argc == 3) env_file = argv[2];
						globalLocalizationDemo(env_file); 
						break;
				case 1:
					fprintf(stderr,"Global localization demo with (40x40x360) area\n");
					if(argc == 3) env_file = argv[2];	
					globalCoarseLocalizationDemo(env_file);
						break;
				case 2:
						fprintf(stderr,"Running a localizer with MCL input\n");
						if(argc == 3) env_file = argv[2]; 
						mclLocalizationDemo(env_file);
						break;
				case 3:
						fprintf(stderr,"Running ML-SLAM Demo\n");
						if(argc == 3) env_file = argv[2]; 
						MLSLAMDemo(env_file);
						break;
			case 4:
				fprintf(stderr,"Running Local ML-SLAM Demo\n");
				if(argc >= 3) scan_meas = argv[2];
				if(argc >= 4) env_out_file = argv[3];
				if(argc >= 5) scan_meas_out = argv[4];
				ML_Local_SLAMDemo(scan_meas, env_out_file,scan_meas_out,false);
				break;
			case 5:
				if(argc >= 3) scan_meas = argv[2];
				if(argc >= 4) env_out_file = argv[3];
				if(argc >= 5) scan_meas_out = argv[4];
				
				ScanMatchDemo(scan_meas ,env_out_file ,scan_meas_out , false);	
				break;
				case 6:
						continousLocalizationDemo(scan_meas);
						break;
										
				/*		
				case 4:
						fprintf(stderr,"Running Global SLAM demo\n"); 
						GlobalSLAMDemo();
						break;*/
				default:
						fprintf(stderr,"Invalid input parameter\n"); 
						break;
		}
		
	
	fprintf(stderr,"Exited\n");	
	return 0;
}
